
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       notify_rgb_led.c
  * @author     baiyang
  * @date       2022-1-16
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "notify_rgb_led.h"
#include "notify.h"

#include <rtthread.h>
#include <rtdevice.h>

#include <uITC/uITC_msg.h>
#include <common/time/gp_time.h>
#include <board_config/borad_config.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
static void _set_rgb(rgb_led_t led, uint8_t red, uint8_t green, uint8_t blue);
static void set_rgb(rgb_led_t led, uint8_t red, uint8_t green, uint8_t blue);
static void update(notify_device_t notify_dev);
/*----------------------------------variable----------------------------------*/
const uint32_t sequence_initialising = DEFINE_COLOUR_SEQUENCE_ALTERNATE(RGB_RED,RGB_BLUE);
const uint32_t sequence_trim_or_esc = DEFINE_COLOUR_SEQUENCE(RGB_RED,RGB_BLUE,RGB_GREEN,RGB_RED,RGB_BLUE,RGB_GREEN,RGB_RED,RGB_BLUE,RGB_GREEN,RGB_BLACK);
const uint32_t sequence_failsafe_leak = DEFINE_COLOUR_SEQUENCE_FAILSAFE(RGB_WHITE);
const uint32_t sequence_failsafe_ekf = DEFINE_COLOUR_SEQUENCE_FAILSAFE(RGB_RED);
const uint32_t sequence_failsafe_gps_glitching = DEFINE_COLOUR_SEQUENCE_FAILSAFE(RGB_BLUE);
const uint32_t sequence_failsafe_radio_or_battery = DEFINE_COLOUR_SEQUENCE_FAILSAFE(RGB_BLACK);

const uint32_t sequence_armed = DEFINE_COLOUR_SEQUENCE_SOLID(RGB_GREEN);
const uint32_t sequence_armed_nogps = DEFINE_COLOUR_SEQUENCE_SOLID(RGB_BLUE);
const uint32_t sequence_prearm_failing = DEFINE_COLOUR_SEQUENCE(RGB_YELLOW,RGB_YELLOW,RGB_BLACK,RGB_BLACK,RGB_YELLOW,RGB_YELLOW,RGB_BLACK,RGB_BLACK,RGB_BLACK,RGB_BLACK);
const uint32_t sequence_disarmed_good_dgps = DEFINE_COLOUR_SEQUENCE_ALTERNATE(RGB_GREEN,RGB_BLACK);
const uint32_t sequence_disarmed_good_gps = DEFINE_COLOUR_SEQUENCE_SLOW(RGB_GREEN);
const uint32_t sequence_disarmed_bad_gps = DEFINE_COLOUR_SEQUENCE_SLOW(RGB_BLUE);

static rt_device_t _dev_vcom;

static struct rgb_led_ops led_ops = {.set_rgb = set_rgb,
                                     ._set_rgb = _set_rgb,
                                     .hw_set_rgb = NULL};

static struct notify_device_ops notify_dev_ops = {.NotifyDeviceDestructor = NULL,
                                                  .init                   = NULL,
                                                  .update                 = update,
                                                  .handle_led_control     = NULL,
                                                  .handle_play_tune       = NULL,
                                                  .play_tune              = NULL,
                                                  .rgb_control            = NULL,
                                                  .rgb_set_id             = NULL};
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void rgb_led_ctor(rgb_led_t led, uint8_t led_off, uint8_t led_bright, uint8_t led_medium, uint8_t led_dim)
{
    rt_memset(led, 0, sizeof(rgb_led_t));

    led->notify_dev.ops = &notify_dev_ops;
    led->ops            = &led_ops;

    led->_led_off = led_off;
    led->_led_bright = led_bright;
    led->_led_medium = led_medium;
    led->_led_dim = led_dim;

    if (_dev_vcom != NULL) {
        _dev_vcom = rt_device_find("serial0");
    }
}

struct rgb_led_ops rgb_led_ops()
{
    return led_ops;
}

struct notify_device_ops rgb_led_notify_device_ops()
{
    return notify_dev_ops;
}

enum rgb_source_t rgb_led_rgb_source(rgb_led_t led)
{
    return (enum rgb_source_t)(((Notify*)led->notify_dev.pNotify)->_rgb_led_override);
}

uint8_t rgb_led_get_brightness(rgb_led_t led)
{
    uint8_t brightness = led->_led_bright;

    switch (((Notify*)led->notify_dev.pNotify)->_rgb_led_brightness) {
    case RGB_LED_OFF:
        brightness = led->_led_off;
        break;
    case RGB_LED_LOW:
        brightness = led->_led_dim;
        break;
    case RGB_LED_MEDIUM:
        brightness = led->_led_medium;
        break;
    case RGB_LED_HIGH:
        brightness = led->_led_bright;
        break;
    }

    // use dim light when connected through USB
    if (_dev_vcom != NULL && brightness > led->_led_dim) {
        rt_bool_t connected;
        rt_device_control(_dev_vcom, RT_DEVICE_CTRL_CONNECT, &connected);

        if (connected) {
            brightness = led->_led_dim;
        }
    }
    return brightness;
}

uint32_t rgb_led_get_colour_sequence_obc()
{
    if (notify_flags.armed) {
        return DEFINE_COLOUR_SEQUENCE_SOLID(RGB_RED);
    }
    return DEFINE_COLOUR_SEQUENCE_SOLID(RGB_GREEN);
}

// _scheduled_update - updates _red, _green, _blue according to notify flags
uint32_t rgb_led_get_colour_sequence()
{
    // initialising pattern
    if (notify_flags.initialising) {
        return sequence_initialising;
    }

    // save trim or any calibration pattern
    if (notify_flags.save_trim ||
        notify_flags.esc_calibration ||
        notify_flags.compass_cal_running ||
        notify_flags.temp_cal_running) {
        return sequence_trim_or_esc;
    }

    // radio and battery failsafe patter: flash yellow
    // gps failsafe pattern : flashing yellow and blue
    // ekf_bad pattern : flashing yellow and red
    if (notify_flags.failsafe_radio ||
        notify_flags.failsafe_gcs ||
        notify_flags.failsafe_battery ||
        notify_flags.ekf_bad ||
        notify_flags.gps_glitching ||
        notify_flags.leak_detected) {

        if (notify_flags.leak_detected) {
            // purple if leak detected
            return sequence_failsafe_leak;
        } else if (notify_flags.ekf_bad) {
            // red on if ekf bad
            return sequence_failsafe_ekf;
        } else if (notify_flags.gps_glitching) {
            // blue on gps glitch
            return sequence_failsafe_gps_glitching;
        }
        // all off for radio or battery failsafe
        return sequence_failsafe_radio_or_battery;
    }

    // solid green or blue if armed
    if (notify_flags.armed) {
        // solid green if armed with GPS 3d lock
        if (notify_flags.gps_status >= uITC_GPS_OK_FIX_3D) {
            return sequence_armed;
        }
        // solid blue if armed with no GPS lock
        return sequence_armed_nogps;
    }

    // double flash yellow if failing pre-arm checks
    if (!notify_flags.pre_arm_check) {
        return sequence_prearm_failing;
    }
    if (notify_flags.gps_status >= uITC_GPS_OK_FIX_3D_DGPS && notify_flags.pre_arm_gps_check) {
        return sequence_disarmed_good_dgps;
    }

    if (notify_flags.gps_status >= uITC_GPS_OK_FIX_3D && notify_flags.pre_arm_gps_check) {
        return sequence_disarmed_good_gps;
    }

    return sequence_disarmed_bad_gps;
}

uint32_t rgb_led_get_colour_sequence_traffic_light(void)
{
    if (notify_flags.initialising) {
        return DEFINE_COLOUR_SEQUENCE(RGB_RED,RGB_GREEN,RGB_BLUE,RGB_RED,RGB_GREEN,RGB_BLUE,RGB_RED,RGB_GREEN,RGB_BLUE,RGB_BLACK);
    }

    if (notify_flags.armed) {
        return DEFINE_COLOUR_SEQUENCE_SLOW(RGB_RED);
    }

    if (brd_safety_switch_state() != SAFETY_DISARMED) {
        if (!notify_flags.pre_arm_check) {
            return DEFINE_COLOUR_SEQUENCE_ALTERNATE(RGB_YELLOW, RGB_BLACK);
        } else {
            return DEFINE_COLOUR_SEQUENCE_SLOW(RGB_YELLOW);
        }
    }

    if (!notify_flags.pre_arm_check) {
        return DEFINE_COLOUR_SEQUENCE_ALTERNATE(RGB_GREEN, RGB_BLACK);
    }
    return DEFINE_COLOUR_SEQUENCE_SLOW(RGB_GREEN);
}

/*
  update LED when in override mode
 */
void rgb_led_update_override(rgb_led_t led)
{
    if (led->_led_override.rate_hz == 0) {
        // solid colour
        rgb_led_set_rgb2(led, led->_led_override.r, led->_led_override.g, led->_led_override.b);
        return;
    }
    // blinking
    uint32_t ms_per_cycle = 1000 / led->_led_override.rate_hz;
    uint32_t cycle = (time_millis() - led->_led_override.start_ms) % ms_per_cycle;
    if (cycle > ms_per_cycle / 2) {
        // on
        rgb_led_set_rgb2(led, led->_led_override.r, led->_led_override.g, led->_led_override.b);
    } else {
        rgb_led_set_rgb2(led, 0, 0, 0);
    }
}

// update - updates led according to timed_updated.  Should be called
// at 50Hz
static void update(notify_device_t notify_dev)
{
    rgb_led_t led = (rgb_led_t)notify_dev;

    uint32_t current_colour_sequence = 0;

    switch (rgb_led_rgb_source(led)) {
    case rgb_mavlink:
        rgb_led_update_override(led);
        return; // note this is a return not a break!
    case rgb_standard:
        current_colour_sequence = rgb_led_get_colour_sequence();
        break;
    case rgb_obc:
        current_colour_sequence = rgb_led_get_colour_sequence_obc();
        break;
    case rgb_traffic_light:
        current_colour_sequence = rgb_led_get_colour_sequence_traffic_light();
        break;
    }

    const uint8_t brightness = rgb_led_get_brightness(led);

    uint8_t step = (time_millis()/100) % 10;

    // ensure we can't skip a step even with awful timing
    if (step != led->last_step) {
        step = (led->last_step+1) % 10;
        led->last_step = step;
    }

    const uint8_t colour = (current_colour_sequence >> (step*3)) & 7;

    uint8_t red_des = (colour & RGB_RED) ? brightness : led->_led_off;
    uint8_t green_des = (colour & RGB_GREEN) ? brightness : led->_led_off;
    uint8_t blue_des = (colour & RGB_BLUE) ? brightness : led->_led_off;

    rgb_led_set_rgb(led, red_des, green_des, blue_des);
}

// set_rgb - set color as a combination of red, green and blue values
static void _set_rgb(rgb_led_t led, uint8_t red, uint8_t green, uint8_t blue)
{
    if (red != led->_red_curr ||
        green != led->_green_curr ||
        blue != led->_blue_curr) {
        // call the hardware update routine
        if (rgb_led_hw_set_rgb(led, red, green, blue)) {
            led->_red_curr = red;
            led->_green_curr = green;
            led->_blue_curr = blue;
        }
    }
}

// set_rgb - set color as a combination of red, green and blue values
static void set_rgb(rgb_led_t led, uint8_t red, uint8_t green, uint8_t blue)
{
    if (rgb_led_rgb_source(led) == rgb_mavlink) {
        // don't set if in override mode
        return;
    }
    rgb_led_set_rgb2(led, red, green, blue);
}

/*------------------------------------test------------------------------------*/


